#ifndef _DEFINE_H_
#define _DEFINE_H_

// Port
// 11, 16, 20 DEAD
#define MOTOR_PORT_LEFT 		5
#define MOTOR_PORT_RIGHT 		3
#define LED_UNDER_FRONTLEFT		10
#define LED_UNDER_FRONTRIGHT	9
#define LED_UNDER_BACKRIGHT		8
#define LED_UNDER_CENTER		7
#define LED_LEFT_FRONT			11
#define LED_LEFT_BACK			13
#define LED_RIGHT_FRONT			14
#define LED_RIGHT_BACK			15
#define GYRO_PORT 				23
#define ENCODER_PORT_LEFT 		24
#define ENCODER_PORT_RIGHT		25
#define SERVO_SCORE				0

// Additional Port
#define BUMPER_WALL_LEFT		17
#define BUMPER_WALL_RIGHT		18
#define BUMPER_FRONT			19
#define BUMPER_BACK				20
#define DISTANCE_PORT			21

// Sharp Distance Parameter
#define DISTANCE_THRESHOLD		6.50

// Gyro
#define LSB_MS_PER_DEG	1062.0
#define GYRO_CALI_TIME	3000

// Variables
uint8_t 	goalPoint;
uint8_t 	pointToDo[3];		// [0] = 4 [1] = 2 [2] = 1
uint8_t 	initPlace;			// 0,1
uint8_t		initOrient;			// 0,1,2,3
uint16_t	threshold_led[8];	// {ufr, ufl, ubr, ufc, lf, lb, rf, rb} -- ufr = Under - Front - Right

// Additional variables
uint8_t		currentSide;		// 0,1,2,3,4 (0 = N, 1 = E, 2 = S, 3 = W, 4 = middle)
uint8_t		currentOrient;		// 0,1,2,3

#endif
